{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing,Matrix\n",
    "init_printing(use_latex='mathjax')\n",
    "from scipy import weave\n",
    "import os\n",
    "import cv2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# X-Tag 一种能够估计位姿的标记\n",
    "---\n",
    "参考文献：X-Tag A Fiducial Tag for Flexible and Accurate Bundle Adjustment\n",
    "\n",
    "形状如下：\n",
    "\n",
    "<img src=\"data/X_Tag.png\" alt=\"X-Tag\"/>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def gen_Base_XTag(a=400):\n",
    "    '''\n",
    "    a: 外框长\n",
    "    r: 大圆半径\n",
    "    '''\n",
    "    img = np.zeros((a,a),np.uint8)\n",
    "    center_x = int(a/2)\n",
    "    center_y = int(a/2)\n",
    "    r = int(a*0.4) # 大外圆\n",
    "    max_r = r\n",
    "    cv2.circle(img,(center_x,center_y),r,(255,0,0),-1)\n",
    "    r = int(a*0.08) # 小内圆\n",
    "    min_r = r\n",
    "    cv2.circle(img,(center_x,center_y),r,(0,0,0),-1)\n",
    "    r = int(a*0.06) # 左右上角\n",
    "    delta_a = int(a*0.125)\n",
    "    cv2.circle(img,(delta_a,delta_a),r,(255,0,0),-1)\n",
    "    cv2.circle(img,(a - delta_a,delta_a),r,(255,0,0),-1)\n",
    "    R = int(0.03*a) # 编码点圆半径\n",
    "    min_r = min_r+R\n",
    "    max_r = max_r-R\n",
    "    return img,min_r,max_r,R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "img,min_r,max_r,R = gen_Base_XTag(500)\n",
    "imshow(img,cmap=\"gray\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def gen_code_XTag(img,R,r,th):\n",
    "    '''\n",
    "    以极坐标在img内画圆\n",
    "    r: 绘制圆新的极坐标(半径)\n",
    "    th: 绘制圆的角度坐标，deg\n",
    "    '''\n",
    "    a = img.shape[0]\n",
    "    center_x = int(a/2)\n",
    "    center_y = int(a/2)\n",
    "    th = deg2rad(th)\n",
    "    x = center_x + r*cos(th)\n",
    "    y = center_y + r*sin(th)\n",
    "    cv2.circle(img,(int(x),int(y)),R,(0,0,0),-1) # 绘制编码点\n",
    "    return img,R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "img,R = gen_code_XTag(img,R,(min_r+max_r)/2.0,60)\n",
    "imshow(img,cmap=\"gray\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _gen_code_XTag(img,R,min_r,max_r,code_list):\n",
    "    '''\n",
    "    随机以极坐标在img内绘制新的点\n",
    "    img: 要绘制的图像\n",
    "    R：绘制的半径\n",
    "    min_r: 要绘制的最小半径\n",
    "    max_r: 要绘制的最大半径\n",
    "    code_list: 已经绘制的点的极坐标列表\n",
    "    '''\n",
    "    delta_r = 2.0*R # 保护间隔范围\n",
    "    min_r = min_r + delta_r\n",
    "    max_r = max_r - delta_r\n",
    "    \n",
    "    a = img.shape[0]\n",
    "    center_x = int(a/2)\n",
    "    center_y = int(a/2)\n",
    "    \n",
    "    def get_xy(r,th):\n",
    "        th = deg2rad(th)\n",
    "        x = center_x + r*cos(th)\n",
    "        y = center_y + r*sin(th)\n",
    "        return int(x),int(y)\n",
    "    \n",
    "    old_xy = []\n",
    "    for old_r,old_th in code_list:\n",
    "        old_x,old_y = get_xy(old_r,old_th)\n",
    "        old_xy.append([old_x,old_y])\n",
    "         \n",
    "    def check(r,th):\n",
    "        if old_xy == []:\n",
    "            return True\n",
    "        for old_x,old_y in old_xy:\n",
    "            x,y = get_xy(r,th)\n",
    "            d = np.linalg.norm([x-old_x,y-old_y])\n",
    "            if d < 2*R + delta_r:\n",
    "                return False\n",
    "        return True\n",
    "    \n",
    "    def gen_r_th():\n",
    "        r = min_r + np.random.random()*(max_r - min_r)\n",
    "        th = np.random.random()*360\n",
    "        return r,th\n",
    "    \n",
    "    while True:\n",
    "        r,th = gen_r_th()\n",
    "        if check(r,th):\n",
    "            x,y = get_xy(r,th)\n",
    "            cv2.circle(img,(int(x),int(y)),R,(0,0,0),-1) # 绘制编码点\n",
    "            return r,th"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def gen_code(R,min_r,max_r,code_list):\n",
    "    delta_r = 0.4*R # 保护间隔范围\n",
    "    min_r = min_r + delta_r\n",
    "    max_r = max_r - delta_r\n",
    "    def check(r,th):\n",
    "        if code_list == []:\n",
    "            return True\n",
    "        for old_r,old_th in code_list:\n",
    "            th1 = abs(np.arccos(R/r))\n",
    "            th2 = abs(np.arccos(R/old_r))\n",
    "            if 0.9*abs(th - old_th) > th1 + th2:\n",
    "                continue\n",
    "            elif abs(r-old_r) > 2*(R + delta_r):\n",
    "                continue\n",
    "            else:\n",
    "                return False\n",
    "        return True\n",
    "    \n",
    "    def gen_r_th():\n",
    "        r = min_r + np.random.random()*(max_r - min_r)\n",
    "        th = np.random.random()*360\n",
    "        return r,th\n",
    "    \n",
    "    while True:\n",
    "        r,th = gen_r_th()\n",
    "        if check(r,th):\n",
    "            break\n",
    "    return r,th"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def gen_XTag(a=400,num=5):\n",
    "    '''\n",
    "    生成XTag\n",
    "    a: 边长\n",
    "    num: 编码点的个数\n",
    "    '''\n",
    "    img,min_r,max_r,R = gen_Base_XTag(a=a)\n",
    "    code_list = []\n",
    "    for i in range(num):\n",
    "        r,th = _gen_code_XTag(img,R,min_r,max_r,code_list)\n",
    "        code_list.append([r,th])\n",
    "    return img"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "imshow(gen_XTag(a=400,num=7),cmap='gray')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def save_XTag(num=10):\n",
    "    for i in range(num):\n",
    "        img = gen_XTag(a=1000,num=7)\n",
    "        cv2.imwrite(\"%d.jpg\" % i,img)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#save_XTag()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
